
#include "pid.h"
#include "bsp_can.h"




// 防止值过大
void limit_value(float *value, int max_value)
{
    if (*value > max_value)
    {
        *value = max_value;
    }
    else if (*value < -max_value)
    {
        *value = -max_value;
    }
}

// 位置式PID
// pid结构体初始化,参数:pid结构体指针， 系数数组, 最大输出， 最大累计误差输出
void pid_init(struct Pid *pid, const float pid_k[3], uint32_t max_out, uint32_t max_iout, uint8_t mode)
{
    // 模式选择
    pid->mode = mode;

    // 系数初始化
    pid->kp = pid_k[0];
    pid->ki = pid_k[1];
    pid->kd = pid_k[2];

    // 输出和累计误差限定,疑问:设置多大合适???
    pid->max_out = max_out;
    pid->max_iout = max_iout;

    // 其余值初始化为0
    pid->err[0] = pid->err[1] = pid->err[2] = pid->out = pid->pout = pid->iout = pid->dout = pid->delta = 0.0;
}


// pid计算，参数:pid结构体指针, 目标值， 实际值
float pid_calculate(struct Pid *pid, float goal, float actual)
{
    // 给结构体传入目标值和实际值
    pid->goal_value = goal;
    pid->actual_value = actual;

    // 误差数组填充
    pid->err[2] = pid->err[1];
    pid->err[1] = pid->err[0];
    pid->err[0] = goal - actual;

    // 位置式pid
    if (pid->mode == 0)
    {
        pid->pout = pid->kp * pid->err[0];
        pid->iout += pid->ki * pid->err[0];
        limit_value(&(pid->iout), pid->max_iout);
        pid->dout = pid->kd * (pid->err[0] - pid->err[1]);

        // 最终输出
        pid->out = pid->pout + pid->iout + pid->dout;
        limit_value(&(pid->out), pid->max_out);

        return pid->out;
    }

    // 增量式pid
    else if (pid->mode == 1)
    {
        pid->pout = pid->kp * (pid->err[0] - pid->err[1]);

        pid->iout = pid->ki * pid->err[0]; // 增量式pid的iout不用限制

        pid->dout = pid->kd * (pid->err[0] - 2 * pid->err[1] + pid->err[2]);

        pid->delta = pid->pout + pid->iout + pid->dout;
        pid->out += pid->delta;
        limit_value(&pid->out, pid->max_out);

        return pid->out;
    }
    else
        return 0;
}

// 以便在调试的时候改变pid系数
void pid_k_set(struct Pid *pid, float kp, float ki, float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
}

//角度结构体初始化,参数:结构体指针,死区值
void angle_init(Angle *angle, float death_angle)
{
    angle->death_angle = death_angle;
    angle->angle_now = angle->angle_last = angle->angle_acc = 0;
    angle->delt[0] = angle->delt[1] = 0;
}

//用于计算绝对角度,参数:角度结构体指针,电机反馈角度
float angle_cal(Angle *angle, float real_angle)
{
    angle->angle_now = real_angle;//更新此次角度
    angle->delt[1] = angle->delt[0];
    angle->delt[0] = angle->angle_now - angle->angle_last;
    angle->angle_last = angle->angle_now;//更新上次角度

    // 两种特殊delt需单独处理
    if (angle->delt[0] < -180)
    { // 正转360->0时
        angle->delt[0] += 360;
    }
    else if (angle->delt[0] > 180)
    { // 反转0->360时
        angle->delt[0] -= 360;
    }
    angle->angle_acc += angle->delt[0];

    return angle->angle_acc;
}

//位置pid串级控制,所需参数:角度结构体指针, 速度环结构体指针, 角度环结构体指针, 目标角度, 实际角度, 实际速度
void pid_pos_cas(Angle *angle, struct Pid *pid_s, struct Pid *pid_p, float set_angle, float real_angle, int real_speed)
{
    float angle_need = 19 * set_angle;
    float angle_acc = 0;
    float speed_need = 0;
    int current_need = 0;

    angle_acc = angle_cal(angle, real_angle);
    speed_need = pid_calculate(pid_p, angle_need, angle_acc);

    //死区控制???
    if (-angle->death_angle < (angle_need - angle_acc) && angle->death_angle > (angle_need - angle_acc)){
      speed_need = 0;
    }

    current_need = (int)pid_calculate(pid_s, speed_need, real_speed);

    can_transmit(current_need, current_need, current_need, current_need);
}


